#include "ntpg.h"
#include <iostream>
#include <iomanip>
#include <cmath>
#include <fstream>
//#include <qwt.h>


using namespace std;

ntpg::ntpg(uint NoImages, uint NoObjectPoints, uint NoCameras) :
	_M(NoImages), _N(NoObjectPoints), _C(NoCameras),
	ik(3*_N+7*_M+3*_C+2*_M*_N), iu(1), xscale(1.0), yscale(1.0), TINY(1e-10), NMAX(1000000)
{
	_T = 3*_N+7*_M+3*_C+2*_M*_N;

	// calculate the integer offsets
	nry = _N;
	nrz = _N+nry;
	ntx = _N+nrz;
	nty = _M+ntx;
	ntz = _M+nty;
	nqw = _M+ntz;
	nqx = _M+nqw;
	nqy = _M+nqx;
	nqz = _M+nqy;
	nvx = _M+nqz;
	nvy = _C+nvx;
	nvz = _C+nvy;
	nux = _C+nvz;
	nuy = _M*_N+nux;

    cout << "_T = " << _T << endl;
    cout << "_M = " << _M << endl;
    cout << "_N = " << _N << endl;
    cout << "_C = " << _C << endl;

	// allocate memory
	_p = new ftype[_T];
	_p = _p-1;
	_k = new uint[_M];
	_k = _k-1;			// _k[1.._M]
	_map = new uint[_T];
	_map = _map-1;			// _map[1.._T]

}

ntpg::~ntpg()
{
	_k=_k+1;
	delete [] _k;
	_p = _p+1;
	delete [] _p;
	_map = _map+1;
	delete [] _map;
}

ftype ntpg::func(ftype* p)
{
	ftype f, fsq, fa, fb;
	ftype q0, q1, q2, q3;
//	ftype sq0, sq1, sq2, sq3, tmp1, tmp2;
	ftype m11, m12, m13, m21, m22, m23, m31, m32, m33;
	ftype wx, wy, wz;
	ftype vz_wz, v_x, v_y, v_z, t_x, t_y, t_z;
	ftype rtx, rty, rtz;
	uint m,n;

	fsq = 0.0;
	for(m=1; m<=_M; m++) {
		t_x = tx(m,p);
		t_y = ty(m,p);
		t_z = tz(m,p);
		v_x = vx(m,p);
		v_y = vy(m,p);
		v_z = vz(m,p);

        q0 = qw(m,p);
        q1 = qx(m,p);
        q2 = qy(m,p);
        q3 = qz(m,p);

        m12 = 2.0*(q1*q2-q0*q3);
        m21 = 2.0*(q1*q2+q0*q3);
        m13 = 2.0*(q1*q3+q0*q2);
        m31 = 2.0*(q1*q3-q0*q2);
        m23 = 2.0*(q2*q3-q0*q1);
        m32 = 2.0*(q2*q3+q0*q1);
        m11 = q0*q0+q1*q1-q2*q2-q3*q3;
        m22 = q0*q0-q1*q1+q2*q2-q3*q3;
        m33 = q0*q0-q1*q1-q2*q2+q3*q3;

		for(n=1; n<=_N; n++) {
			rtx = rx(n,p)-t_x;
			rty = ry(n,p)-t_y;
			rtz = rz(n,p)-t_z;
			wx = m11*rtx+m12*rty+m13*rtz;
			wy = m21*rtx+m22*rty+m23*rtz;
			wz = m31*rtx+m32*rty+m33*rtz;
			vz_wz = v_z/wz;
			fa = ux(m,n,p)-v_x+vz_wz*wx;
			fb = uy(m,n,p)-v_y+vz_wz*wy;
			fsq += fa*fa+fb*fb;
		}
	}
	f = sqrt(fsq);
//	cout << "error = " << eps << endl;
	return f;
}

ftype ntpg::dfunc(ftype p[], ftype df[])
{
	// allocate memory
	ftype f, fsq, fa, fb;
	ftype v_x, v_y, v_z, t_x, t_y, t_z;
	ftype q0, q1, q2, q3;
//	ftype sq0, sq1, sq2, sq3, tmp1, tmp2;
	ftype m11, m12, m13, m21, m22, m23, m31, m32, m33;
	ftype rtx, rty, rtz;
	ftype wx, wy, wz;
	ftype wx_wz, wy_wz, vz_wz;
	ftype dwx, dwy, dwz;
//	ftype daux, dauy, davx, davy, davz;
//	ftype dbux, dbuy, dbvx, dbvy, dbvz;
//	ftype darx, dary, darz, datx, daty, datz, daqw, daqx, daqy, daqz;
//	ftype dbrx, dbry, dbrz, dbtx, dbty, dbtz, dbqw, dbqx, dbqy, dbqz;
	ftype da, db;
	uint m,n,i;

	// This may not be necessary if vector df is already initialized to 0
	for(i=1; i<=_U; i++)
		df[i] = 0.0;
	fsq = 0.0;
	for(m=1; m<=_M; m++) {	// image index
        t_x = tx(m,p);
        t_y = ty(m,p);
        t_z = tz(m,p);
        v_x = vx(m,p);
        v_y = vy(m,p);
        v_z = vz(m,p);

        q0 = qw(m,p);
        q1 = qx(m,p);
        q2 = qy(m,p);
        q3 = qz(m,p);

        m12 = 2.0*(q1*q2-q0*q3);
        m21 = 2.0*(q1*q2+q0*q3);
        m13 = 2.0*(q1*q3+q0*q2);
        m31 = 2.0*(q1*q3-q0*q2);
        m23 = 2.0*(q2*q3-q0*q1);
        m32 = 2.0*(q2*q3+q0*q1);
        m11 = q0*q0+q1*q1-q2*q2-q3*q3;
        m22 = q0*q0-q1*q1+q2*q2-q3*q3;
        m33 = q0*q0-q1*q1-q2*q2+q3*q3;

        for(n=1; n<=_N; n++) {	// object index
            rtx = rx(n,p)-t_x;
            rty = ry(n,p)-t_y;
            rtz = rz(n,p)-t_z;
            wx = m11*rtx+m12*rty+m13*rtz;
            wy = m21*rtx+m22*rty+m23*rtz;
            wz = m31*rtx+m32*rty+m33*rtz;
            vz_wz = v_z/wz;
            wx_wz = wx/wz;
            wy_wz = wy/wz;
            fa = ux(m,n,p)-v_x+vz_wz*wx;
            fb = uy(m,n,p)-v_y+vz_wz*wy;
            fsq += fa*fa+fb*fb;

			// differentiate with respect to ux
			if(iux(m,n)<=_U){
//				daux = 1.0;
//				dbux = 0.0;
//				ux(m,n,df) += fa*daux+fb*dbux;
				ux(m,n,df) += fa;
			}
			// differentiate with respect to uy
			if(iuy(m,n)<=_U){
//				dauy = 0.0;
//				dbuy = 1.0;
//				uy(m,n,df) += fa*dauy+fb*dbuy;
				uy(m,n,df) += fb;
			}
			// differentiate with respect to rx
			if(irx(n)<=_U){
				da=vz_wz*(m11-m31*wx_wz);
				db=vz_wz*(m21-m31*wy_wz);
				rx(n,df) += fa*da+fb*db;
			}
			// differentiate with respect to ry
			if(iry(n)<=_U){
				da=vz_wz*(m12-m32*wx_wz);
				db=vz_wz*(m22-m32*wy_wz);
				ry(n,df) += fa*da+fb*db;
			}
			// differentiate with respect to rz
			if(irz(n)<=_U){
				da=vz_wz*(m13-m33*wx_wz);
				db=vz_wz*(m23-m33*wy_wz);
				rz(n,df) += fa*da+fb*db;
			}
			// differentiate with respect to tx
			if(itx(m)<=_U){
//				datx=-darx;
//				dbtx=-dbrx;
				da=-vz_wz*(m11-m31*wx_wz);
				db=-vz_wz*(m21-m31*wy_wz);
				tx(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to ty
			if(ity(m)<=_U){
//				daty=-dary;
//				dbty=-dbry;
				da=-vz_wz*(m12-m32*wx_wz);
				db=-vz_wz*(m22-m32*wy_wz);
				ty(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to tz
			if(itz(m)<=_U){
//				datz=-darz;
//				dbtz=-dbrz;
				da=-vz_wz*(m13-m33*wx_wz);
				db=-vz_wz*(m23-m33*wy_wz);
				tz(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to vx
			if(ivx(m)<=_U){
//				davx=-1.0;
//				dbvx=0.0;
//				vx(m,df) += fa*davx+fb*dbvx;
				vx(m,df) -= fa;
			}
			// differentiate with respect to vy
			if(ivy(m)<=_U){
//				davy=0.0;
//				dbvy=-1.0;
//				vy(m,df) += fa*davy+fb*dbvy;
				vy(m,df) -= fb;
			}
			// differentiate with respect to vz
			if(ivz(m)<=_U){
//				davz=wx_wz;
//				dbvz=wy_wz;
//				vz(m,df) += fa*davz+fb*dbvz;
				vz(m,df) += fa*wx_wz+fb*wy_wz;
			}
			// differentiate with respect to qw
			if(iqw(m)<=_U){
				dwx =  q0*rtx - q3*rty + q2*rtz;
				dwy =  q3*rtx + q0*rty - q1*rtz;
				dwz = -q2*rtx + q1*rty + q0*rtz;
				da = 2.0*vz_wz*(dwx-wx_wz*dwz);
				db = 2.0*vz_wz*(dwy-wy_wz*dwz);
				qw(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to qx
			if(iqx(m)<=_U){
				dwx =  q1*rtx + q2*rty + q3*rtz;
				dwy =  q2*rtx - q1*rty - q0*rtz;
				dwz =  q3*rtx + q0*rty - q1*rtz;
				da = 2.0*vz_wz*(dwx-wx_wz*dwz);
				db = 2.0*vz_wz*(dwy-wy_wz*dwz);
				qx(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to qy
			if(iqy(m)<=_U){
				dwx = -q2*rtx + q1*rty + q0*rtz;
				dwy =  q1*rtx + q2*rty + q3*rtz;
				dwz = -q0*rtx + q3*rty - q2*rtz;
				da = 2.0*vz_wz*(dwx-wx_wz*dwz);
				db = 2.0*vz_wz*(dwy-wy_wz*dwz);
				qy(m,df) += fa*da+fb*db;
			}
			// differentiate with respect to qz
			if(iqz(m)<=_U){
				dwx = -q3*rtx - q0*rty + q1*rtz;
				dwy =  q0*rtx - q3*rty + q2*rtz;
				dwz =  q1*rtx + q2*rty + q3*rtz;
				da = 2.0*vz_wz*(dwx-wx_wz*dwz);
				db = 2.0*vz_wz*(dwy-wy_wz*dwz);
				qz(m,df) += fa*da+fb*db;
			}
		}
	}
	f = sqrt(fsq);
	for(i=1; i<=_U; i++)
		df[i] = df[i]/f;

	return f;
}

ftype ntpg::Hessian(ftype p[], ftype** h)
{
	ftype f, fsq, fa, fb;
	ftype v_x, v_y, v_z, t_x, t_y, t_z;
	ftype q0, q1, q2, q3;
	ftype rtx, rty, rtz;
	ftype m11, m12, m13, m21, m22, m23, m31, m32, m33;
	ftype wx, wy, wz, wx_wz, wy_wz, vz_wz;
	ftype dwxqw, dwyqw, dwzqw, dwxqx, dwyqx, dwzqx, dwxqy, dwyqy, dwzqy, dwxqz, dwyqz, dwzqz;
	// variables to calculate gradient
	ftype daux, dauy, darx, dary, darz, datx, daty, datz, davx, davy, davz, daqw, daqx, daqy, daqz;
	ftype dbux, dbuy, dbrx, dbry, dbrz, dbtx, dbty, dbtz, dbvx, dbvy, dbvz, dbqw, dbqx, dbqy, dbqz;
	// variables to calculate Hessian
	ftype davzrx, davzry,	davzrz,	davztx,	davzty,	davztz,	davzqw,	davzqx,	davzqy,	davzqz,	davzvz,	darxrx,	daryry,	darzrz,	datxtx,	datyty,	datztz,	datxty,	datxtz,	datytz,	datxry,	datxrz,	datyrz, darxtx,	daryty,	darztz,	darxry,	darxrz,	daryrz,	darxty,	darxtz,	darytz,	daqwqw,	daqxqx,	daqyqy,	daqzqz,	daqwqx,	daqwqy,	daqwqz,	daqxqy,	daqxqz,	daqyqz,	daqwrx,	daqwry,	daqwrz,	daqxrx,	daqxry,	daqxrz,	daqyrx,	daqyry,	daqyrz,	daqzrx,	daqzry,	daqzrz,	daqwtx,	daqwty,	daqwtz,	daqxtx,	daqxty,	daqxtz,	daqytx,	daqyty,	daqytz,	daqztx,	daqzty,	daqztz;
	ftype dbvzrx, dbvzry,	dbvzrz,	dbvztx,	dbvzty,	dbvztz,	dbvzqw,	dbvzqx,	dbvzqy,	dbvzqz,	dbvzvz,	dbrxrx,	dbryry,	dbrzrz,	dbtxtx,	dbtyty,	dbtztz,	dbtxty,	dbtxtz,	dbtytz,	dbtxry,	dbtxrz,	dbtyrz, dbrxtx,	dbryty,	dbrztz,	dbrxry,	dbrxrz,	dbryrz,	dbrxty,	dbrxtz,	dbrytz,	dbqwqw,	dbqxqx,	dbqyqy,	dbqzqz,	dbqwqx,	dbqwqy,	dbqwqz,	dbqxqy,	dbqxqz,	dbqyqz,	dbqwrx,	dbqwry,	dbqwrz,	dbqxrx,	dbqxry,	dbqxrz,	dbqyrx,	dbqyry,	dbqyrz,	dbqzrx,	dbqzry,	dbqzrz,	dbqwtx,	dbqwty,	dbqwtz,	dbqxtx,	dbqxty,	dbqxtz,	dbqytx,	dbqyty,	dbqytz,	dbqztx,	dbqzty,	dbqztz;

	uint m,n,i,j;

	// This may not be necessary if vector h is already initialized
	for(i=1; i<=_U; i++)
		for(j=1; j<=_U; j++)
			h[i][j] = 0.0;
	fsq = 0.0;
	for(m=1; m<=_M; m++) {	// image index
        t_x = tx(m,p);
        t_y = ty(m,p);
        t_z = tz(m,p);
        v_x = vx(m,p);
        v_y = vy(m,p);
        v_z = vz(m,p);

        q0 = qw(m,p);
        q1 = qx(m,p);
        q2 = qy(m,p);
        q3 = qz(m,p);

        m12 = 2.0*(q1*q2-q0*q3);
        m21 = 2.0*(q1*q2+q0*q3);
        m13 = 2.0*(q1*q3+q0*q2);
        m31 = 2.0*(q1*q3-q0*q2);
        m23 = 2.0*(q2*q3-q0*q1);
        m32 = 2.0*(q2*q3+q0*q1);
        m11 = q0*q0+q1*q1-q2*q2-q3*q3;
        m22 = q0*q0-q1*q1+q2*q2-q3*q3;
        m33 = q0*q0-q1*q1-q2*q2+q3*q3;

        for(n=1; n<=_N; n++) {	// object index
            rtx = rx(n,p)-t_x;
            rty = ry(n,p)-t_y;
            rtz = rz(n,p)-t_z;
            wx = m11*rtx+m12*rty+m13*rtz;
            wy = m21*rtx+m22*rty+m23*rtz;
            wz = m31*rtx+m32*rty+m33*rtz;
            vz_wz = v_z/wz;
            wx_wz = wx/wz;
            wy_wz = wy/wz;
            fa = ux(m,n,p)-v_x+vz_wz*wx;
            fb = uy(m,n,p)-v_y+vz_wz*wy;
            fsq += fa*fa+fb*fb;

			dwxqw =  2.0*( q0*rtx - q3*rty + q2*rtz);
			dwyqw =  2.0*( q3*rtx + q0*rty - q1*rtz);
			dwzqw =  2.0*(-q2*rtx + q1*rty + q0*rtz);

			dwxqx =  2.0*( q1*rtx + q2*rty + q3*rtz);
			dwyqx =  2.0*( q2*rtx - q1*rty - q0*rtz);
			dwzqx =  2.0*( q3*rtx + q0*rty - q1*rtz);

			dwxqy =  2.0*(-q2*rtx + q1*rty + q0*rtz);
			dwyqy =  2.0*( q1*rtx + q2*rty + q3*rtz);
			dwzqy =  2.0*(-q0*rtx + q3*rty - q2*rtz);

			dwxqz =  2.0*(-q3*rtx - q0*rty + q1*rtz);
			dwyqz =  2.0*( q0*rtx - q3*rty + q2*rtz);
			dwzqz =  2.0*( q1*rtx + q2*rty + q3*rtz);

			// gradient(fa)
			daux = 1.0;
			dauy = 0.0;
			darx=vz_wz*(m11-m31*wx_wz);
			dary=vz_wz*(m12-m32*wx_wz);
			darz=vz_wz*(m13-m33*wx_wz);
			datx=-darx;
			daty=-dary;
			datz=-darz;
			davx=-1.0;
			davy=0.0;
			davz=wx_wz;
			daqw = 2.0*vz_wz*(dwxqw-wx_wz*dwzqw);
			daqx = 2.0*vz_wz*(dwxqx-wx_wz*dwzqx);
			daqy = 2.0*vz_wz*(dwxqy-wx_wz*dwzqy);
			daqz = 2.0*vz_wz*(dwxqz-wx_wz*dwzqz);

			// Hessian(fa)
			davzrx = darx/v_z;
			davzry = dary/v_z;
			davzrz = darz/v_z;
			davztx = datx/v_z;
			davzty = daty/v_z;
			davztz = datz/v_z;
			davzqw = daqw/v_z;
			davzqx = daqx/v_z;
			davzqy = daqy/v_z;
			davzqz = daqz/v_z;
			davzvz = 0.0;

			daqwqw = -2.0*(daqw*dwzqw/wz + vz_wz*( rtx - wx_wz*rtz));
			daqxqx = -2.0*(daqx*dwzqx/wz + vz_wz*( rtx + wx_wz*rtz));
			daqyqy = -2.0*(daqy*dwzqy/wz + vz_wz*(-rtx + wx_wz*rtz));
			daqzqz = -2.0*(daqz*dwzqz/wz + vz_wz*(-rtx - wx_wz*rtz));

			daqwqx = -(daqw*dwzqx + daqx*dwzqw)/wz + 2.0*vz_wz*(-wx_wz*rty);
			daqwqy = -(daqw*dwzqy + daqy*dwzqw)/wz + 2.0*vz_wz*(rtz + wx_wz*rtx);
			daqwqz = -(daqw*dwzqz + daqz*dwzqw)/wz + 2.0*vz_wz*(-rty);
			daqxqy = -(daqx*dwzqx + daqx*dwzqx)/wz + 2.0*vz_wz*(rty);
			daqxqz = -(daqx*dwzqy + daqy*dwzqx)/wz + 2.0*vz_wz*(rtz - wx_wz*rtx);
			daqyqz = -(daqy*dwzqz + daqz*dwzqy)/wz + 2.0*vz_wz*(-wx_wz*rty);

			daqwrx = -(daqw*m31 + darx*dwzqw)/wz + 2.0*vz_wz*( q0 + wx_wz*q2);
			daqwry = -(daqw*m32 + dary*dwzqw)/wz + 2.0*vz_wz*(-q3 - wx_wz*q1);
			daqwrz = -(daqw*m33 + darz*dwzqw)/wz + 2.0*vz_wz*( q2 - wx_wz*q0);
			daqxrx = -(daqx*m31 + darx*dwzqx)/wz + 2.0*vz_wz*( q1 - wx_wz*q3);
			daqxry = -(daqx*m32 + dary*dwzqx)/wz + 2.0*vz_wz*( q2 - wx_wz*q0);
			daqxrz = -(daqx*m33 + darz*dwzqx)/wz + 2.0*vz_wz*( q3 + wx_wz*q1);
			daqyrx = -(daqy*m31 + darx*dwzqy)/wz + 2.0*vz_wz*(-q2 + wx_wz*q0);
			daqyry = -(daqy*m32 + dary*dwzqy)/wz + 2.0*vz_wz*( q1 - wx_wz*q3);
			daqyrz = -(daqy*m33 + darz*dwzqy)/wz + 2.0*vz_wz*( q0 + wx_wz*q2);
			daqzrx = -(daqz*m31 + darx*dwzqz)/wz + 2.0*vz_wz*(-q3 - wx_wz*q1);
			daqzry = -(daqz*m32 + dary*dwzqz)/wz + 2.0*vz_wz*(-q0 - wx_wz*q2);
			daqzrz = -(daqz*m33 + darz*dwzqz)/wz + 2.0*vz_wz*( q1 - wx_wz*q3);

			daqwtx = -daqwrx;
			daqwty = -daqwry;
			daqwtz = -daqwrz;
			daqxtx = -daqxrx;
			daqxty = -daqxry;
			daqxtz = -daqxrz;
			daqytx = -daqyrx;
			daqyty = -daqyry;
			daqytz = -daqyrz;
			daqztx = -daqzrx;
			daqzty = -daqzry;
			daqztz = -daqzrz;

			darxrx = datxtx = -2.0/wz*darx*m31;
			daryry = datyty = -2.0/wz*dary*m32;
			darzrz = datztz = -2.0/wz*darz*m33;
			darxtx = -darxrx;
			daryty = -daryry;
			darztz = -darzrz;
			darxry = datxty = -(darx*m32 + dary*m31)/wz;
			darxrz = datxtz = -(darx*m33 + darz*m31)/wz;
			daryrz = datytz = -(dary*m33 + darz*m32)/wz;
			darxty = datxry = -darxry;
			darxtz = datxrz = -darxrz;
			darytz = datyrz = -daryrz;


			// gradient(fb)
			dbux = 0.0;
			dbuy = 1.0;
			dbrx=vz_wz*(m21-m31*wy_wz);
			dbry=vz_wz*(m22-m32*wy_wz);
			dbrz=vz_wz*(m23-m33*wy_wz);
			dbtx=-dbrx;
			dbty=-dbry;
			dbtz=-dbrz;
			dbvx=0.0;
			dbvy=-1.0;
			dbvz=wy_wz;
			dbqw = 2.0*vz_wz*(dwyqw-wy_wz*dwzqw);
			dbqx = 2.0*vz_wz*(dwyqx-wy_wz*dwzqx);
			dbqy = 2.0*vz_wz*(dwyqy-wy_wz*dwzqy);
			dbqz = 2.0*vz_wz*(dwyqz-wy_wz*dwzqz);

			// Hessian(fb)
			dbvzrx = dbrx/v_z;
			dbvzry = dbry/v_z;
			dbvzrz = dbrz/v_z;
			dbvztx = dbtx/v_z;
			dbvzty = dbty/v_z;
			dbvztz = dbtz/v_z;
			dbvzqw = dbqw/v_z;
			dbvzqx = dbqx/v_z;
			dbvzqy = dbqy/v_z;
			dbvzqz = dbqz/v_z;
			dbvzvz = 0.0;

			dbqwqw = -2.0*(dbqw*dwzqw/wz + vz_wz*( rty - wy_wz*rtz));
			dbqxqx = -2.0*(dbqx*dwzqx/wz + vz_wz*(-rty + wy_wz*rtz));
			dbqyqy = -2.0*(dbqy*dwzqy/wz + vz_wz*( rty + wy_wz*rtz));
			dbqzqz = -2.0*(dbqz*dwzqz/wz + vz_wz*(-rty - wy_wz*rtz));

			dbqwqx = -(dbqw*dwzqx + dbqx*dwzqw)/wz + 2.0*vz_wz*(-rtz-wy_wz*rty);
			dbqwqy = -(dbqw*dwzqy + dbqy*dwzqw)/wz + 2.0*vz_wz*(wy_wz*rtx);
			dbqwqz = -(dbqw*dwzqz + dbqz*dwzqw)/wz + 2.0*vz_wz*(rtx);
			dbqxqy = -(dbqx*dwzqx + dbqx*dwzqx)/wz + 2.0*vz_wz*(rtx);
			dbqxqz = -(dbqx*dwzqy + dbqy*dwzqx)/wz + 2.0*vz_wz*(-wy_wz*rtx);
			dbqyqz = -(dbqy*dwzqz + dbqz*dwzqy)/wz + 2.0*vz_wz*(rtz-wy_wz*rty);

			dbqwrx = -(dbqw*m31 + dbrx*dwzqw)/wz + 2.0*vz_wz*( q3 + wy_wz*q2);
			dbqwry = -(dbqw*m32 + dbry*dwzqw)/wz + 2.0*vz_wz*( q0 - wy_wz*q1);
			dbqwrz = -(dbqw*m33 + dbrz*dwzqw)/wz + 2.0*vz_wz*(-q1 - wy_wz*q0);
			dbqxrx = -(dbqx*m31 + dbrx*dwzqx)/wz + 2.0*vz_wz*( q2 - wy_wz*q3);
			dbqxry = -(dbqx*m32 + dbry*dwzqx)/wz + 2.0*vz_wz*(-q1 - wy_wz*q0);
			dbqxrz = -(dbqx*m33 + dbrz*dwzqx)/wz + 2.0*vz_wz*(-q0 + wy_wz*q1);
			dbqyrx = -(dbqy*m31 + dbrx*dwzqy)/wz + 2.0*vz_wz*( q1 + wy_wz*q0);
			dbqyry = -(dbqy*m32 + dbry*dwzqy)/wz + 2.0*vz_wz*( q2 - wy_wz*q3);
			dbqyrz = -(dbqy*m33 + dbrz*dwzqy)/wz + 2.0*vz_wz*( q3 + wy_wz*q2);
			dbqzrx = -(dbqz*m31 + dbrx*dwzqz)/wz + 2.0*vz_wz*( q0 - wy_wz*q1);
			dbqzry = -(dbqz*m32 + dbry*dwzqz)/wz + 2.0*vz_wz*(-q3 - wy_wz*q2);
			dbqzrz = -(dbqz*m33 + dbrz*dwzqz)/wz + 2.0*vz_wz*( q2 - wy_wz*q3);

			dbqwtx = -dbqwrx;
			dbqwty = -dbqwry;
			dbqwtz = -dbqwrz;
			dbqxtx = -dbqxrx;
			dbqxty = -dbqxry;
			dbqxtz = -dbqxrz;
			dbqytx = -dbqyrx;
			dbqyty = -dbqyry;
			dbqytz = -dbqyrz;
			dbqztx = -dbqzrx;
			dbqzty = -dbqzry;
			dbqztz = -dbqzrz;

			dbrxrx = dbtxtx = -2.0/wz*dbrx*m31;
			dbryry = dbtyty = -2.0/wz*dbry*m32;
			dbrzrz = dbtztz = -2.0/wz*dbrz*m33;
			dbrxtx = -dbrxrx;
			dbryty = -dbryry;
			dbrztz = -dbrzrz;
			dbrxry = dbtxty = -(dbrx*m32 + dbry*m31)/wz;
			dbrxrz = dbtxtz = -(dbrx*m33 + dbrz*m31)/wz;
			dbryrz = dbtytz = -(dbry*m33 + dbrz*m32)/wz;
			dbrxty = dbtxry = -dbrxry;
			dbrxtz = dbtxrz = -dbrxrz;
			dbrytz = dbtyrz = -dbryrz;

			// partibl derivative
//			ux(m,n,h) += fa*daux+fb*dbux;
//			uy(m,n,h) += fa*dauy+fb*dbuy;
//			rx(n,h) += fa*darx+fb*dbrx;
//			ry(n,h) += fa*dary+fb*dbry;
//			rz(n,h) += fa*darz+fb*dbrz;
//			tx(m,h) += fa*datx+fb*dbtx;
//			ty(m,h) += fa*daty+fb*dbty;
//			tz(m,h) += fa*datz+fb*dbtz;
//			vx(m,h) += fa*davx+fb*dbvx;
//			vy(m,h) += fa*davy+fb*dbvy;
//			vz(m,h) += fa*davz+fb*dbvz;
//			qw(m,h) += fa*daqw+fb*dbqw;
//			qx(m,h) += fa*daqx+fb*dbqx;
//			qy(m,h) += fa*daqy+fb*dbqy;
//			qz(m,h) += fa*daqz+fb*dbqz;
		}
	}
	f = sqrt(fsq);
	for(i=1; i<=_T; i++)
		for(j=1; j<=_T; j++)
			h[i][j] *= f;
	return f;
}

// functions to change variables without initially setting the state
void ntpg::setr(uint i, ftype v1, ftype v2, ftype v3)
{
	setrx(i,v1);
	setry(i,v2);
	setrz(i,v3);
}
void ntpg::sett(uint i, ftype v1, ftype v2, ftype v3)
{
	settx(i,v1);
	setty(i,v2);
	settz(i,v3);
}
void ntpg::setv(uint i, ftype v1, ftype v2, ftype v3)
{
	setvx(i,v1);
	setvy(i,v2);
	setvz(i,v3);
}
void ntpg::setq(uint i, ftype v0, ftype v1, ftype v2, ftype v3)
{
	setqw(i,v0);
	setqx(i,v1);
	setqy(i,v2);
	setqz(i,v3);
}
void ntpg::setu(uint i1, uint i2, ftype v1, ftype v2)
{
	setux(i1,i2,v1);
	setuy(i1,i2,v2);
}

// functions to set variables to initial values
void ntpg::setrx(uint i, ftype value, bool state)
{
	irx(i) = state ? ik-- : iu++;
	setrx(i,value);
}
void ntpg::setry(uint i, ftype value, bool state)
{
	iry(i) = state ? ik-- : iu++;
	setry(i,value);
}
void ntpg::setrz(uint i, ftype value, bool state)
{
	irz(i) = state ? ik-- : iu++;
	setrz(i,value);
}
void ntpg::settx(uint i, ftype value, bool state)
{
	itx(i) = state ? ik-- : iu++;
	settx(i,value);
}
void ntpg::setty(uint i, ftype value, bool state)
{
	ity(i) = state ? ik-- : iu++;
	setty(i,value);
}
void ntpg::settz(uint i, ftype value, bool state)
{
	itz(i) = state ? ik-- : iu++;
	settz(i,value);
}
void ntpg::setqw(uint i, ftype value, bool state)
{
	iqw(i) = state ? ik-- : iu++;
	setqw(i,value);
}
void ntpg::setqx(uint i, ftype value, bool state)
{
	iqx(i) = state ? ik-- : iu++;
	setqx(i,value);
}
void ntpg::setqy(uint i, ftype value, bool state)
{
	iqy(i) = state ? ik-- : iu++;
	setqy(i,value);
}
void ntpg::setqz(uint i, ftype value, bool state)
{
	iqz(i) = state ? ik-- : iu++;
	setqz(i,value);
}
void ntpg::setvx(uint i, ftype value, bool state)
{
	ivx(i) = state ? ik-- : iu++;
	setvx(i,value);
}
void ntpg::setvy(uint i, ftype value, bool state)
{
	ivy(i) = state ? ik-- : iu++;
	setvy(i,value);
}
void ntpg::setvz(uint i, ftype value, bool state)
{
	ivz(i) = state ? ik-- : iu++;
	setvz(i,value);
}
void ntpg::setux(uint i1, uint i2, ftype value, bool state)
{
	iux(i1,i2) = state ? ik-- : iu++;
	setux(i1,i2,value);
}
void ntpg::setuy(uint i1, uint i2, ftype value, bool state)
{
	iuy(i1,i2) = state ? ik-- : iu++;
	setuy(i1,i2,value);
}
void ntpg::setr(uint i, ftype v1, ftype v2, ftype v3, bool state)
{
	setrx(i,v1,state);
	setry(i,v2,state);
	setrz(i,v3,state);
}
void ntpg::sett(uint i, ftype v1, ftype v2, ftype v3, bool state)
{
	settx(i,v1,state);
	setty(i,v2,state);
	settz(i,v3,state);
}
void ntpg::setq(uint i, ftype v0, ftype v1, ftype v2, ftype v3, bool state)
{
	setqw(i,v0,state);
	setqx(i,v1,state);
	setqy(i,v2,state);
	setqz(i,v3,state);
}
void ntpg::setv(uint i, ftype v1, ftype v2, ftype v3, bool state)
{
	setvx(i,v1,state);
	setvy(i,v2,state);
	setvz(i,v3,state);
}
void ntpg::setu(uint i1, uint i2, ftype v1, ftype v2, bool state)
{
	setux(i1,i2,v1,state);
	setuy(i1,i2,v2,state);
}
void ntpg::setfocalplaneresolution(ftype xResolution, ftype yResolution)
{
	xscale = 1.0/xResolution;
	yscale = 1.0/yResolution;
}
void ntpg::getQuat(uint m, ftype *p, ftype& q0, ftype& q1, ftype& q2, ftype& q3)
{
	q0 = qw(m,p);
	q1 = qx(m,p);
	q2 = qy(m,p);
	q3 = qz(m,p);
}
void ntpg::normQuat(uint m, ftype *p, ftype scale)
{
	ftype q0, q1, q2, q3, nrm;
	getQuat(m, p, q0, q1, q2, q3);
	nrm = sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
	if(nrm > 0.0) {
		qw(m) = scale*q0/nrm;
		qx(m) = scale*q1/nrm;
		qy(m) = scale*q2/nrm;
		qz(m) = scale*q3/nrm;
	}
}
void ntpg::getRotMatrix(uint m, ftype *p, ftype& m11, ftype& m12, ftype& m13, ftype& m21, ftype& m22, ftype& m23, ftype& m31, ftype& m32, ftype& m33)
{
	ftype q0, q1, q2, q3;
	ftype sq0, sq1, sq2, sq3, tmp1, tmp2;

	getQuat(m,p,q0,q1,q2,q3);

	sq0 = q0*q0;
	sq1 = q1*q1;
	sq2 = q2*q2;
	sq3 = q3*q3;

	m11 = sq0+sq1-sq2-sq3;
	m22 = sq0-sq1+sq2-sq3;
	m33 = sq0-sq1-sq2+sq3;

	tmp1 = q1*q2;
	tmp2 = q3*q0;
	m21 = 2.0 * (tmp1 + tmp2);
	m12 = 2.0 * (tmp1 - tmp2);

	tmp1 = q1*q3;
	tmp2 = q2*q0;
	m31 = 2.0 * (tmp1 - tmp2);
	m13 = 2.0 * (tmp1 + tmp2);

	tmp1 = q2*q3;
	tmp2 = q1*q0;
	m32 = 2.0 * (tmp1 + tmp2);
	m23 = 2.0 * (tmp1 - tmp2);

/*
	// Euler angles
	m11 = cz*cy;
	m12 = sx*sy*cz+cx*sz;
	m13 = -cx*sy*cz+sx*sz;
	m21 = -cy*sz;
	m22 = -sx*sy*sz+cx*cz;
	m23 = cx*sy*sz+sx*cz;
	m31 = sy;
	m32 = -sx*cy;
	m33 = cx*cy;
//*/

}
void ntpg::globalImagePoint(uint m, ftype ux, ftype uy, ftype& x, ftype& y, ftype &z)
{
	ftype m11,m12,m13,m21,m22,m23,m31,m32,m33,x0,y0,z0;
	x0 = ux-vx(m);
	y0 = uy-vy(m);
	z0 = -vz(m);

	getRotMatrix(m,m11,m12,m13,m21,m22,m23,m31,m32,m33);
	x = m11*x0+m21*y0+m31*z0+tx(m);
	y = m12*x0+m22*y0+m32*z0+ty(m);
	z = m13*x0+m23*y0+m33*z0+tz(m);
}
void ntpg::globalImagePixel(uint m, uint ux, uint uy, ftype& x, ftype& y, ftype &z)
{
	globalImagePoint(m, (ftype)ux*xscale, (ftype)uy*yscale, x, y, z);
}
void ntpg::orientImage(uint m, ftype wx, ftype wy, ftype wz, ftype angle_rot, bool state)
{
	double lw, nx, ny, nz, ux, uy, uz, lu, vx, vy, vz, lv;
	double a11, a12, a13, a21, a22, a23, a31, a32, a33;
	double b11, b12, b13, b21, b22, b23, b31, b32, b33;
	double m11, m12, m13, m21, m22, m23, m31, m32, m33;
	double q0, q1, q2, q3, sq0, sq1, sq2, sq3;
	double c1, s1;
	double lwx, lwy, lwz;
	const double pi = 4.0*atan(1.0);

	// vec(w) is the z axis of image m
	// angle_rot is the rotation angle around the z axis

	// normalize vec(w)
	lw = sqrt(wx*wx+wy*wy+wz*wz);
	if(lw > 0.0) {
		wx /= lw;
		wy /= lw;
		wz /= lw;
	}

	// the image needs to be oriented with respect to the global coordinate system
	// choose the axis that is most orthogonal to vec(w)
	lwx = sqrt(wx*wx);
	lwy = sqrt(wy*wy);
	lwz = sqrt(wz*wz);
	if(lwx <= lwy && lwx <= lwz) {
		nx = 1.0;
		ny = 0.0;
		nz = 0.0;
	} else if(lwy <= lwz) {
		nx = 0.0;
		ny = 1.0;
		nz = 0.0;
	} else {
		nx = 0.0;
		ny = 0.0;
		nz = 1.0;
	}

	// calculate vec(v) orthogonal to vec(w) and then normalize
	vx = wy*nz-wz*ny;
	vy = wz*nx-wx*nz;
	vz = wx*ny-wy*nx;
	lv = sqrt(vx*vx+vy*vy+vz*vz);
	if(lv > 0.0) {
		vx /= lv;
		vy /= lv;
		vz /= lv;
	}

	// calculate vec(u) orthogonal to vec(w) and vec(v) and then normzlize
	ux = vy*wz-vz*wy;
	uy = vz*wx-vx*wz;
	uz = vx*wy-vy*wx;

	lu = sqrt(ux*ux+uy*uy+uz*uz);
	if(lu > 0.0) {
		ux /= lu;
		uy /= lu;
		uz /= lu;
	}

	// construct rotation matrix
	a11 = ux; a12 = uy; a13 = uz;
	a21 = vx; a22 = vy; a23 = vz;
	a31 = wx; a32 = wy; a33 = wz;

	// rotate around axis vec(w) by angle_rot
	c1 = cos(-angle_rot/2.0*pi/180.0);
	s1 = sin(-angle_rot/2.0*pi/180.0);

	q0 = c1;
	q1 = s1*wx;
	q2 = s1*wy;
	q3 = s1*wz;

	sq0 = q0*q0;
	sq1 = q1*q1;
	sq2 = q2*q2;
	sq3 = q3*q3;

	// use b11 and b22 as temp variables
	b11 = q0*q3;
	b22 = q1*q2;
	b12 = 2.0*(b22-b11);
	b21 = 2.0*(b22+b11);

	b11 = q0*q2;
	b22 = q1*q3;
	b13 = 2.0*(b22+b11);
	b31 = 2.0*(b22-b11);

	b11 = q0*q1;
	b22 = q2*q3;
	b23 = 2.0*(b22-b11);
	b32 = 2.0*(b22+b11);

	// calulate diagonal terms last since m11 and m12 was used as temp variables
	b11 = sq0+sq1-sq2-sq3;
	b22 = sq0-sq1+sq2-sq3;
	b33 = sq0-sq1-sq2+sq3;

	// combine rotations
	m11 = a11*b11+a12*b21+a13*b31;
	m12 = a11*b12+a12*b22+a13*b32;
	m13 = a11*b13+a12*b23+a13*b33;
	m21 = a21*b11+a22*b21+a23*b31;
	m22 = a21*b12+a22*b22+a23*b32;
	m23 = a21*b13+a22*b23+a23*b33;
	m31 = a31*b11+a32*b21+a33*b31;
	m32 = a31*b12+a32*b22+a33*b32;
	m33 = a31*b13+a32*b23+a33*b33;

	// convert matrix to quaternion
	float trace = m11 + m22 + m33 + 1.0;
	if( trace > 1e-6 ) {
		double s = 0.5 / sqrt(trace);
		q0 = 0.25 / s;
		q1 = ( m32 - m23 ) * s;
		q2 = ( m13 - m31 ) * s;
		q3 = ( m21 - m12 ) * s;
	} else {
		if ( m11 > m22 && m11 > m33 ) {
			double s = 2.0 * sqrt( 1.0 + m11 - m22 - m33);
			q0 = (m23 - m32 ) / s;
			q1 = 0.25 * s;
			q2 = (m12 + m21 ) / s;
			q3 = (m13 + m31 ) / s;
		} else if (m22 > m33) {
			double s = 2.0 * sqrt( 1.0 + m22 - m11 - m33);
			q0 = (m13 - m31 ) / s;
			q1 = (m12 + m21 ) / s;
			q2 = 0.25 * s;
			q3 = (m23 + m32 ) / s;
		} else {
			double s = 2.0 * sqrt( 1.0 + m33 - m11 - m22 );
			q0 = (m12 - m21 ) / s;
			q1 = (m13 + m31 ) / s;
			q2 = (m23 + m32 ) / s;
			q3 = 0.25 * s;
		}
	}
	setq(m, q0, q1, q2, q3, state);
}

void ntpg::orientImage(uint m, ftype theta, ftype phi, ftype angle_rot, bool state)
{
	double wx, wy, wz;
	wx = sin(theta)*cos(phi);
	wy = sin(theta)*sin(phi);
	wz = cos(theta);
	orientImage(m, wx, wy, wz, angle_rot, state);
}

void ntpg::solve(ftype ftol)
{
	if(ik!=iu-1) {
		cout << "Error in setting variables" << endl;
		cout << "Solver did not start" << endl;
		return;
	}
	_U = ik;
	cout << "_U = " << _U << endl;
	cout << "initial error = " << func(_p) << endl;

/*
	cout << "Using steepest descent solver" << endl;
	uint iter;
	steepestdescent(_p, ftol, ftol, iter);
//*/

/*
	cout << "Using frprmn solver" << endl;
	frprmn(_p);
//*/

//*
	cout << "Using conjugate gradient polak ribiere solver" << endl;
	uint iter;
	cgpr(_p, ftol, ftol, iter);
//*/

/*
	cout << "Using conjugate gradient fletcher reeves solver" << endl;
	uint iter;
	cgfr(_p, ftol, ftol, iter);
//*/
/*
	cout << "Using simplex solver" << endl;
	ftype **p, *y, lam;
	uint m, n;
	// allocate memory for y[1.._U]
	y = new ftype[_U+1];
	y = y-1;				// p[1.._U]

	// allocate memory for p[1.._U+1,1.._T]
	p = new ftype*[_U+1];
	p = p-1;
	for(m=1; m<=_U+1; m++) {
		p[m] = new ftype[_T];
		p[m] = p[m]-1;
	}

	// init matrix(p)
	lam = 120.0;
	for(n=1; n<=_T; n++) {
		p[1][n] = _p[n];
	}
	for(m=2; m<=_U+1; m++) {
		for(n=1; n<=_T; n++) {
			p[m][n] = _p[n];
			if(m==n) {
				p[m][n] += lam;
			}
		}
	}

	// init vec(y)
	for(m=1; m<=_U+1; m++) {
		y[m] = func(p[m]);
	}

	uint nfunc;
	amoeba(p, y, _U, ftol, nfunc);

	cout << "initial error is = " << func(_p) << endl;
	cout << "number of function evaluations = " << nfunc << endl;
	cout << "error = " << y[1] << endl;
	for(n=1; n<=_U; n++) {
		_p[n] = p[1][n];
	}
	cout << "final error = " << func(_p) << endl;


	for(m=1; m<=_U+1; m++) {
		p[m] = p[m]+1;
		delete[] p[m];
	}

	p = p + 1;
	delete[] p;

	y = y+1;
	delete[] y;
//*/

//	showQuats(false);
//	showV(false);
//	showT(false);
//	showR(true);
//	showU(true);
//	linmin(_p, -60.0, 60.0, 10000);

//*
	for(uint m=1; m<=_M; m++) {
		normQuat(m,3.0);
	}
//	cout << "the error after quaternion normalization = " << func(_p) << endl;
//*/
}

void ntpg::linmin(ftype *x, ftype smin, ftype smax, uint N)
{
	ofstream file ("linefunc.m");
	if (file.is_open()) {

		// allocate memeory
		ftype *d, *g, *x0;
		ftype f, ds, a, h, s0;
		uint i;

		d = new ftype[_U];
		g = new ftype[_U];
		x0 = new ftype[_T];

		d = d-1;
		g = g-1;
		x0 = x0-1;

		for(i=_U+1; i<=_T; i++)
			x0[i] = x[i];

		// calculate gradient
		ftype f0;
		f0 = dfunc(x,d);
		h = 0.0;
		for(i=1; i<=_U; i++) {
			d[i] = -d[i];
			h += d[i]*d[i];
		}
		s0 = 2.0*f0/h;

		cout << "f0 = " << f0 << endl;
		cout << "h = " << h << endl;
		cout << "s0 = " << s0 << endl;


		ds = (smax-smin)/ftype(N);
		file << "m = [" << endl;
		for(a=smin; a<=smax; a+=ds)
		{
			for(i=1; i<=_U; i++)
				x0[i] = x[i] + a*d[i];
			f = func(x0);
			// calculate gradient at x0
			dfunc(x0,g);
			// calculate derivative of line function evaluated at x0
			h=0.0;
			for(i=1; i<=_U; i++)
				h += g[i]*d[i];

			file << setiosflags(ios::fixed) << setprecision(10);
			file << a << ", " << f << ", " << h << endl;

		}
		file << "];" << endl;


		// find the minumum using the secant method
//*
        ftype eps2, dd, hp;
        uint it, j, jmax;
		jmax = 10;
		eps2 = 1e-4;

//		ftype f0;
		f0 = dfunc(x,d);
		dd = 0.0;
		for(it=1; it<=_U; it++){
			d[it] = -d[it];
			dd += d[it]*d[it];
		}
		// for the photogrammetry problem it seems a good intital staring point for s0 is:
		// calculate the derivative of the line function at alpha = 0, fit a line, and then
		// see where line crosses the x axis.  Double this distance to get s0.
		s0 = 2.0*f0/dd;
		cout << "s0 = " << s0 << endl;
		a=-s0;
		for(it=1; it<=_U; it++)
			x0[it] = x[it]+s0*d[it];
		dfunc(x0, g);
		hp=0.0;
		for(it=1; it<=_U; it++)
			hp += g[it]*d[it];
		j=0;
		do{
			dfunc(x, g);
			h=0.0;
			for(it=1; it<=_U; it++)
				h += g[it]*d[it];
			a*=h/(hp-h);
			for(it=1; it<=_U; it++)
				x[it] += a*d[it];
			hp = h;
			j++;
			cout << "alpha[" << j << "] = " << a << endl;
		}while(j<jmax && a*a*dd>eps2*eps2);

		cout << "number of iterations = " << j << endl;
		cout << "alpha min = " << a << endl;
		for(it=1; it<=_U; it++)
			x0[it] = x[it]+a*d[it];
		cout << "f(" << a << ") = " << func(x0) << endl;
		cout << "f(" << 0.0 << ") = " << func(x) << endl;

//*/
		x0 = x0+1;
		g = g+1;
		d = d+1;
		delete[] x0;
		delete[] g;
		delete[] d;

		file.close();
	}
}

void ntpg::cgpr(ftype x[], ftype eps1, ftype eps2, uint &iter)
{
	// Given a function f, a starting value x, a maximum number of CG iterations imax, a CG
	// error tolerance eps1 < 1, a Secant method step parameter s0, a maximum number of Secant
	// method iterations jmax, and a secant method error tolerance eps2 < 1:
	uint i=1, j, k=0, it, imax=15, jmax=10;
	ftype *r, *d, *g, *x0, *s, dnew, dmid, dold, d0, dd, a, b, s0, h, hp, f0, f1;

	iter = 0;

	r = new ftype[_U];
	d = new ftype[_U];
	g = new ftype[_U];
	s = new ftype[_U];
	x0 = new ftype[_T];
	r = r-1;
	d = d-1;
	g = g-1;
	s = s-1;
	x0 = x0-1;

	// copy the known variables so that when func(x0) gets called it calculates the correct error
	for(it=_U+1; it<=_T; it++)
		x0[it] = x[it];

	// r <== -f'(x)
	f1=dfunc(x,r);
	for(it=1; it<=_U; it++)
		r[it] = -r[it];
	// calculate a preconditioner M ~ f''(x)
	// s <== M^-1 r
	// s <== r
	for(it=1; it<=_U; it++)
		s[it] = r[it];
	// d <== s
	for(it=1; it<=_U; it++)
		d[it] = s[it];

	dnew=0.0;
	for(it=1; it<=_U; it++)
		dnew += r[it]*d[it];
	d0=dnew;
    do{
//	while(i<imax && dnew>eps1*eps1*d0) {
        // plot the line function
        cout << "iter = " << i << endl;
        f0 = f1;
		dd = 0.0;
		for(it=1; it<=_U; it++)
			dd += d[it]*d[it];
		s0 = 2.0*f0/dd;
		cout << "s0 = " << s0 << endl;

		// for dubuggin purposes
        if(i==1) {
				cout << "output for i="<< i << endl;
        		ofstream file ("linfunc.m");
				if (file.is_open()) {
					ftype smin=10.0, smax=40.0, ds, ff, hh, aa;
					uint N = 100, ii;
					ds = (smax-smin)/ftype(N);
					file << "m = [" << endl;
					for(aa=smin; aa<=smax; aa+=ds)
					{
						for(ii=1; ii<=_U; ii++)
							x0[ii] = x[ii] + aa*d[ii];
						// calculate gradient at x0
						ff = dfunc(x0,g);
						// calculate derivative of line function evaluated at x0
						hh=0.0;
						for(ii=1; ii<=_U; ii++)
							hh += g[ii]*d[ii];

						file << setiosflags(ios::fixed) << setprecision(10);
						file << aa << ", " << ff << ", " << hh << endl;

					}
					file << "];" << endl;

				}
        }
        // end debugging;
		a=-s0;
		for(it=1; it<=_U; it++)
			x0[it] = x[it]+s0*d[it];
		dfunc(x0, g);
		hp=0.0;
		for(it=1; it<=_U; it++)
			hp += g[it]*d[it];
		j=0;
		do{
			dfunc(x, g);
			h=0.0;
			for(it=1; it<=_U; it++)
				h += g[it]*d[it];
			a*=h/(hp-h);
			for(it=1; it<=_U; it++)
				x[it] = x[it] + a*d[it];
			hp = h;
			j++;
			iter++;
			cout << "alpha[" << j << "] = " << a << endl;
		}while(j<jmax && a*a*dd>eps2*eps2);
		f1 = dfunc(x, r);
		for(it=1; it<=_U; it++)
			r[it] = -r[it];
		dold=dnew;

		// dmid <==r^t s
		dmid = 0.0;
		for(it=1; it<=_U; it++)
			dmid += r[it]*s[it];
		// s <== r
		for(it=1; it<=_U; it++)
			s[it] = r[it];
		// dnew = r^t s
		dnew = 0;
		for(it=1; it<=_U; it++)
			dnew += r[it]*s[it];
		b = (dnew-dmid)/dold;
		k++;
		if(k==_U || b<=0.0) {
			for(it=1; it<=_U; it++)
				d[it]=s[it];
			k=0;
		} else {
			for(it=1; it<=_U; it++)
				d[it]=s[it]+b*d[it];
		}
		i++;
//		cout << "dnew =" << dnew << endl;
		cout << "cost = " << f0 << endl;
		cout << endl;
	}while(i<imax && (2.0*fabs(f1-f0)>=eps1*(fabs(f1)+fabs(f0)+1.0e-10)));

	cout << "number line minimizations:  " << iter << endl;
	cout << "number of CG iterations " << i << endl;
	cout << "average number of line minimizations per iteration: " << float(iter)/float(i) << endl;
	cout << "error = " << func(x) << endl;
	x0 = x0+1;
	g = g+1;
	d = d+1;
	r = r+1;
	s = s+1;
	delete[] x0;
	delete[] g;
	delete[] d;
	delete[] r;
	delete[] s;
}

void ntpg::cgfr(ftype x[], ftype eps1, ftype eps2, uint &iter)
{
	// Given a function f, a starting value x, a maximum number of CG iterations imax, a CG
	// error tolerance eps1 < 1, a Secant method step parameter s0, a maximum number of Secant
	// method iterations jmax, and a secant method error tolerance eps2 < 1:
	uint i=0, j, k=0, it, imax=1000000, jmax=10;
	ftype *r, *d, *g, *x0, dnew, dold, d0, dd, a, b, s0, h, hp, rtd, cost;

	iter = 0;
	s0 = 0.001;  // inital sigma value

	r = new ftype[_U];
	d = new ftype[_U];
	g = new ftype[_U];
	x0 = new ftype[_T];
	r = r-1;
	d = d-1;
	g = g-1;
	x0 = x0-1;

	for(it=_U+1; it<=_T; it++)
		x0[it] = x[it];
	// r <== -f'(x)
	// d <== r
	dfunc(x,r);
	for(it=1; it<=_U; it++)
		r[it] = d[it] = -r[it];

	dnew=0.0;
	for(it=1; it<=_U; it++)
		dnew += r[it]*r[it];
	d0=dnew;
	while(i<imax && dnew>eps1*eps1*d0) {
		dd = 0.0;
		for(it=1; it<=_U; it++)
			dd += d[it]*d[it];
		a=-s0;
		for(it=1; it<=_U; it++)
			x0[it] = x[it]+s0*d[it];
		dfunc(x0, g);
		hp=0.0;
		for(it=1; it<=_U; it++)
			hp += g[it]*d[it];
		j=0;
		do{
			dfunc(x, g);
			h=0.0;
			for(it=1; it<=_U; it++)
				h += g[it]*d[it];
			a*=h/(hp-h);
			for(it=1; it<=_U; it++)
				x[it] = x[it] + a*d[it];
			hp = h;
			j++;
			iter++;
//			cout << "alpha[" << j << "] = " << a << endl;
		}while(j<jmax && a*a*dd>eps2*eps2);
		cost = dfunc(x, r);
		for(it=1; it<=_U; it++)
			r[it] = -r[it];
		dold=dnew;
		// dnew <==r^t r
		dnew = 0.0;
		for(it=1; it<=_U; it++)
			dnew += r[it]*r[it];
		b = dnew/dold;
		for(it=1; it<=_U; it++)
			d[it] = r[it]+b*d[it];
		k++;
		rtd = 0.0;
		for(it=1; it<=_U; it++)
			rtd += r[it]*d[it];
		if(k==_U || rtd<=0.0) {
			for(it=1; it<=_U; it++)
				d[it]=r[it];
			k=0;
		}
		i++;
//		cout << "dnew =" << dnew << endl;
		cout << "cost = " << cost << endl;
	}
	cout << "number line minimizations:  " << iter << endl;
	cout << "number of CG iterations " << i << endl;
	cout << "average number of line minimizations per iteration: " << float(iter)/float(i) << endl;
	cout << "error = " << func(x) << endl;
	x0 = x0+1;
	g = g+1;
	d = d+1;
	r = r+1;
	delete[] x0;
	delete[] g;
	delete[] d;
	delete[] r;
}

void ntpg::amoeba(ftype **p, ftype y[], uint ndim, ftype ftol, uint &nfunk)
{
    //Multidimensional minimization of the function func(x) where x[1..ndim] is a vector in ndim dimensions, by the downhill simplex method of Nelder and Mead.
    //The matrix p[1..ndim+1] [1..ndim] is input. Its ndim+1 rows are ndim-dimensional vectors which are the vertices of the starting simplex.
    //Also input is the vector y[1..ndim+1], whose components must be preinitialized to the values of func evaluated at the ndim+1 vertices (rows) of p;
    //and ftol the fractional convergence tolerance to be achieved in the function value (n.b.!). On output, p and y will have been reset to ndim+1 new points
    //all within ftol of a minimum function value, and nfunk gives the number of function evaluations taken.
	uint i,ihi,ilo,inhi,j,mpts=ndim+1;
	ftype rtol,sum,swap,ysave,ytry,*psum;
	psum = new ftype[_T];
	psum = psum-1;
	nfunk=0;

	// need to copy the rest of the vector so that when func() gets called it calculates the correct error
	for(j=1; j<=_T; j++)
		psum[j] = _p[j];


	// GET_PSUM
	for (j=1;j<=ndim;j++) {
		for (sum=0.0,i=1;i<=mpts;i++)
			sum += p[i][j];
		psum[j]=sum;
	}

	for (;;) {
		ilo=1;
		//First we must determine which pouint is the highest (worst), next-highest, and lowest (best), by looping over the pouints in the simplex.
		ihi = y[1]>y[2] ? (inhi=2,1) : (inhi=1,2);
		for (i=1;i<=mpts;i++) {
			if (y[i] <= y[ilo]) ilo=i;
			if (y[i] > y[ihi]) {
				inhi=ihi;
				ihi=i;
			} else if (y[i] > y[inhi] && i != ihi) inhi=i;
		}
		rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo])+TINY);
		// Compute the fractional range from highest to lowest and return if satisfactory.
		if (rtol < ftol) { // If returning, put best pouint and value in slot 1.
			//SWAP(y[1],y[ilo])
			swap = y[1];
			y[1] = y[ilo];
			y[ilo] = swap;
			for (i=1;i<=ndim;i++) {
				swap=p[1][i];
				p[1][i]=p[ilo][i];
				p[ilo][i]=swap;
			}
			break;
		}
		if (nfunk >= NMAX)
			cout << "NMAX exceeded in simplex solver" << endl;
		nfunk += 2;
		// Begin a new iteration. First extrapolate by a factor  1 through the face of the simplex across from the high pouint, i.e., reflect the simplex from the high pouint.
		ytry=amotry(p,y,psum,ndim,ihi,-1.0);
		if (ytry <= y[ilo]) // Gives a result better than the best pouint, so try an additional extrapolation by a factor 2.
			ytry=amotry(p,y,psum,ndim,ihi,2.0);
		else if (ytry >= y[inhi]) { // The reflected pouint is worse than the second-highest, so look for an uintermediate lower pouint, i.e., do a one-dimensional contraction.
			ysave=y[ihi];
			ytry=amotry(p,y,psum,ndim,ihi,0.5);
			if (ytry >= ysave) { // Can t seem to get rid of that high pouint. Better contract around the lowest (best) pouint.
				for (i=1;i<=mpts;i++) {
					if (i != ilo) {
						for (j=1;j<=ndim;j++)
							p[i][j]=psum[j]=0.5*(p[i][j]+p[ilo][j]);
						y[i]=func(psum);
					}
				}
				nfunk += ndim; // Keep track of function evaluations.
				// GET_PSUM  Recompute psum.
				for (j=1;j<=ndim;j++) {
					for (sum=0.0,i=1;i<=mpts;i++)
						sum += p[i][j];
					psum[j]=sum;
				}
			}
		} else
			--nfunk; // Correct the evaluation count.
	} //Go back for the test of doneness and the next iteration.
	psum = psum+1;
	delete[] psum;
}

//Extrapolates by a factor fac through the face of the simplex across from the high pouint, tries it, and replaces the high point if the new point is better.
ftype ntpg::amotry(ftype **p, ftype y[], ftype psum[], uint ndim, uint ihi, ftype fac)
{
	uint j;
	ftype fac1,fac2,ytry,*ptry;
	ptry=new ftype[_T];
	ptry = ptry-1;
	fac1=(1.0-fac)/ndim;
	fac2=fac1-fac;
	for (j=1;j<=ndim;j++)
		ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
	// need to copy the rest of the vector so that when func() gets called it calculates the correct error
	for(j=ndim+1; j<=_T; j++)
		ptry[j] = _p[j];
	ytry=func(ptry);
	//	Evaluate the function at the trial pouint.
	if (ytry < y[ihi]) { //If s is better than the highest, then replace the highest.
		y[ihi]=ytry;
		for (j=1;j<=ndim;j++) {
			psum[j] += ptry[j]-p[ihi][j];
			p[ihi][j]=ptry[j];
		}
	}
	ptry = ptry+1;
	delete[] ptry;
	return ytry;
}
void ntpg::showMap()
{

	cout << "map = [" << endl;
	for(uint i=1; i<=_T; i++) {
		cout << i << '\t' << _map[i] << endl;
	}
	cout << "]" << endl;
	cout << "iu = " << iu << endl;
	cout << "ik = " << ik << endl;
}

void ntpg::showQuats(bool state)
{
	for(uint m=1; m<=_M; m++) {
		normQuat(m);
		cout << "solver->setq("<< m << ", " << qw(m) << ", " << qx(m) << ", " << qy(m) << ", " << qz(m) << ", " << state << ");" << endl;
	}
}
void ntpg::showV(bool state)
{
	for(uint m=1; m<=_M; m++) {
		cout << "solver->setv("<< m << ", " << vx(m) << ", " << vy(m) << ", " << vz(m) << ", " << state << ");" << endl;
	}
}

void ntpg::showR(bool state)
{
	for(uint n=1; n<=_N; n++) {
		cout << "solver->setr("<< n << ", " << rx(n) << ", " << ry(n) << ", " << rz(n) << ", " << state << ");" << endl;
	}

}

void ntpg::showT(bool state)
{	for(uint m=1; m<=_M; m++) {
		cout << "solver->sett("<< m << ", " << tx(m) << "," << ty(m) << ", " << tz(m) << ", " << state << ");" << endl;
	}
}

void ntpg::showU(bool state)
{
	for(uint m=1; m<=_M; m++) {
		for(uint n=1; n<=_N; n++) {
			cout << "solver->setu(" << m << ", " << n << ", " << ux(m,n)/xscale << ", " << uy(m,n)/yscale << ", " << state << ");" << endl;
		}
		cout << endl;
	}

}

void ntpg::steepestdescent(ftype x[], ftype eps1, ftype eps2, uint &iter)
{
	// Given a function f, a starting value x, a maximum number of CG iterations imax, a CG
	// error tolerance eps1 < 1, a Secant method step parameter s0, a maximum number of Secant
	// method iterations jmax, and a secant method error tolerance eps2 < 1:
	uint i=0, j, it, imax=300000, jmax=10;
	ftype *r, *d, *g, *x0, dnew, d0, dd, a, s0, h, hp;

	iter = 0;
	s0 = 1.0;  // inital sigma value

	r = new ftype[_U];
	d = new ftype[_U];
	g = new ftype[_U];
	x0 = new ftype[_T];
	r = r-1;
	d = d-1;
	g = g-1;
	x0 = x0-1;

	for(it=_U+1; it<=_T; it++)
		x0[it] = x[it];

	// r <== -f'(x)
	dfunc(x,r);
	for(it=1; it<=_U; it++)
		r[it] = d[it] = -r[it];
	// calculate a preconditioner M ~ f''(x)
	// s <== M^-1 r


	dnew=0.0;
	for(it=1; it<=_U; it++)
		dnew += r[it]*r[it];
	d0=dnew;
	while(i<imax && dnew>eps1*eps1*d0) {
		dd=0.0;
		for(it=1; it<=_U; it++)
			dd += d[it]*d[it];
		a=-s0;
		for(it=1; it<=_U; it++)
			x0[it] = x[it]+s0*d[it];
		dfunc(x0, g);
		hp=0.0;
		for(it=1; it<=_U; it++)
			hp += g[it]*d[it];
		j=0;
		do{
			dfunc(x, g);
			h=0.0;
			for(it=1; it<=_U; it++)
				h += g[it]*d[it];
			a*=h/(hp-h);
			for(it=1; it<=_U; it++)
				x[it] += a*d[it];
			hp = h;
			j++;
			iter++;
		}while(j<jmax && a*a*dd>eps2*eps2);
		dfunc(x, r);
		for(it=1; it<=_U; it++)
			d[it] = -r[it];
		dnew = 0;
		for(it=1; it<=_U; it++)
			dnew += r[it]*r[it];
		i++;
//        cout << "dnew =" << dnew << endl;
	}
	cout << "number line minimizations:  " << iter << endl;
	cout << "number of iterations " << i << endl;
	cout << "average number of line minimizations per iteration: " << float(iter)/float(i) << endl;
	cout << "error = " << func(x) << endl;
	x0 = x0+1;
	g = g+1;
	d = d+1;
	r = r+1;
	delete[] x0;
	delete[] g;
	delete[] d;
	delete[] r;
}
